#include <iostream>
#include <stdio.h>
#include "opencv2/core.hpp"
#include "opencv2/core/utility.hpp"
#include "opencv2/core/ocl.hpp"
#include "opencv2/imgcodecs.hpp"
#include "opencv2/highgui.hpp"
#include "opencv2/features2d.hpp"
#include "opencv2/calib3d.hpp"
#include "opencv2/imgproc.hpp"
#include"opencv2/flann.hpp"
#include"opencv2/xfeatures2d.hpp"
#include"opencv2/ml.hpp"

#include <common.h>

using namespace cv;
using namespace std;
using namespace cv::xfeatures2d;
using namespace cv::ml;



int main(int argc ,char ** argv)
{
	bool g_step=false ;
    int frameToStart = 0;
	if(argc<3){
	 cout<<"usage ./swtichMon -i vieofile [-j frame_id ] [--step]"<<endl;
	 return -1 ;
	}
	VideoCapture capture(argv[2]);
	if(argc>=5){
		string skip_frame_id(argv[4]);
		stringstream ss;
		ss<<skip_frame_id;
		ss>>frameToStart;
		cout<<"设置开始帧："<<frameToStart<<endl;
	}
	if(argc==6){
		string step(argv[5]);
		if(step=="--step"){
			g_step = true ;
		}
	}
	if(!capture.isOpened()){
	 cout<<"fail to open "<<argv[2]<<endl;
	 return -1 ;
	}
	Mat frame;
	Mat last_frame;
	int frame_cnt=0;

	Ptr<SURF> surf;      //创建方式和2中的不一样
	surf = SURF::create(800);


    capture.set( CV_CAP_PROP_POS_FRAMES,frameToStart);
    cout<<"从第"<<frameToStart<<"帧开始读"<<endl;
    frame_cnt = frameToStart;

    cv::Mat accumulative_frame = cv::Mat(ANA_HEIGHT,ANA_WIDTH, CV_8UC3, Scalar::all(255));
	while(1)
	{

	 if(!capture.read(frame))
	 {
		 cout<<"读取视频失败"<<endl;
		 break;
	 }

	 cv::cvtColor(frame, frame, CV_BGR2GRAY);
	 cv::resize(frame, frame, cv::Size(800, 600), (0, 0), (0, 0), cv::INTER_LINEAR);
	 cout<<"读取帧："<<frame_cnt<<endl;
	 imshow("origin_frame",frame);

	 if(frame_cnt>1){


		if(frame.type()!=last_frame.type()){
			cout<<"skip frame: "<<frame_cnt<<endl;
			cout<<frame.type()<<" "<<last_frame.type()<<endl;
			last_frame = frame.clone();
			continue;
		}
		BFMatcher matcher;
		Mat descriptor_frame, descriptor_last_frame;
		vector<KeyPoint>key1, key2;
		vector<DMatch> matches;

		surf->detectAndCompute(frame, Mat(), key1, descriptor_frame);
		surf->detectAndCompute(last_frame, Mat(), key2, descriptor_last_frame);

		matcher.match(descriptor_frame, descriptor_last_frame, matches);       //匹配
		cout<<"match size: "<<matches.size()<<endl;
//
//			    sort(matches.begin(), matches.end());  //筛选匹配点
//			    vector< DMatch > good_matches;
//			    int ptsPairs = std::min(50, (int)(matches.size() * 0.15));
//			    cout << ptsPairs << endl;
//			    for (int i = 0; i < ptsPairs; i++)
//			    {
//			        good_matches.push_back(matches[i]);
//			    }

		Mat keyPointsFrame;
		drawKeypoints(frame,key1,keyPointsFrame,
				Scalar(0,0,255),DrawMatchesFlags::DRAW_RICH_KEYPOINTS);
		for (size_t i = 0; i < matches.size(); i++)
		{
			double dis = getDistance(key2[matches[i].queryIdx].pt.x ,key1[matches[i].queryIdx].pt.x);
			cout<<"point Dis:"<<dis<<endl;
			if(dis>200)
			{
				line(accumulative_frame,key1[matches[i].queryIdx].pt,key1[matches[i].queryIdx].pt
						,Scalar(0, 255, 0), 2, LINE_AA);
			}

			line(frame,key1[matches[i].queryIdx].pt,key1[matches[i].queryIdx].pt
					,Scalar(0, 255, 0), 2, LINE_AA);

		}
		imshow(" frame key points ",keyPointsFrame);
		imshow(" trace ",accumulative_frame);
		imshow(" now frame increment ",frame);
	 }
	 last_frame = frame.clone();

		frame_cnt++;
		if(g_step){
			waitKey(0);
		}else{
			waitKey(1);
		}
	}
	capture.release();
	return 0;
}
